#include "WProgram.h"

#define MAGIC_CODE 0xf3

enum OPERATION { SET_WIDTH=1, DO_PULSE };

static unsigned int pulse_width = 1500;

struct servcontrol
{
	unsigned char magic;
	unsigned char operation;
	unsigned int param;
};

void setup()
{
	pinMode(2, OUTPUT);
	Serial.begin(9600);
}

inline void send_ready()
{
	Serial.print('\x01');
}
	
inline void do_pulse()
{
	digitalWrite(2, HIGH);
	delayMicroseconds(pulse_width);
	digitalWrite(2, LOW);
	delay(10);
}

inline void got_command(struct servcontrol *sc)
{
	unsigned int i;

	switch(sc->operation) {
		case SET_WIDTH:
			pulse_width = sc->param;
			break;
		case DO_PULSE:
			for(i = 0; i < sc->param; i++)
				do_pulse();

			send_ready();
			break;
	}
}

void loop()
{
	struct servcontrol sc;
	char *b = (char *)&sc;
	unsigned char i;

	if(Serial.available() >= sizeof(struct servcontrol)) {
		for(i = 0; i < sizeof(struct servcontrol); i++)
			*b++ = (char)Serial.read();

		if(sc.magic == MAGIC_CODE)
			got_command(&sc);
	}
}

int main()
{
	init();
	setup();

	while(1)
		loop();
}
